
#ifndef CURRENT_MONITOR_H_
#define CURRENT_MONITOR_H_

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <functional>
#include "chassis_controller/odometry.h"
#include "chassis_controller/pivots.h"
#include <agv_msgs/msg/agv_event.hpp>
//#include "chassis_controller.h"

namespace chassis_controller
{ 
  namespace bacc = boost::accumulators;

  struct monitor_param{
    bool    enable_current_monitor = false;
    double  imbalance_threshold = 2.0;
    int     current_monitor_time = 20;
  };
  
  class CurrentMonitor
  {
  public:
     CurrentMonitor();

    void init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, 
      std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::AGVEvent>> event_pub, 
      const rclcpp::Time &time, std::vector<Spivot_value> &pivot_value);

    /// @brief 使用位置反馈计算车体中心位置
    /// @param pivot_value  各驱动单元的实际值
    /// @param pivot_param  各驱动单元的参数
    /// @param time 
    /// @return 正确计算
    void update(std::vector<Spivot_value> &pivot_value, 
      std::vector<Spivot_param> &pivot_param, 
      agv_msgs::msg::AGVEvent& event_msg, 
      bool standstill, 
      bool reset_button,monitor_param &param);


    // void setRollingWindowSize(size_t velocity_rolling_window_size);
    double meanFilter(double input , std::vector<double> &vector,
      double start_time,double time_Window);
    
  private:
    void pubError();
    // typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMean;
    // typedef bacc::tag::rolling_window RollingWindow;  
    // size_t rolling_window_size_;
    // std::vector<RollingMean> current_accu_vec_;
    // std::vector<double> current_mean_vec_;
    std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_handle_;
    std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::AGVEvent>> event_pub_;
    //ros::Subscriber IO_sub_;
    std::vector<double> current_vec_;

    std::vector<std::vector<double>> current_mean_vec_;
    //RollingMean current_accu_;
    // void resetAccumulators();
    //agv_msgs::msg::AGVEvent event_msg_;
    int pivot_num_;
    double standstill_time_;
    double event_pub_time_;
    double moving_time_;
    bool check_ready_;
    bool reset_button_;

    
    /**
     * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
     */
    // void integrateRungeKutta2(double linear, double angular);

    /**
     * \brief Integrates the velocities (linear and angular) using exact method
     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
     */
    // void integrateExact(double linear, double angular);

    Sxya frameTrans(Sxya original , double angle);

    /// Current timestamp:
    rclcpp::Time timestamp_;

    /// Current pose in map:
    Sxya pos_vehicle_;    

    /// Current pose in map:
    Sxya position_; 

    /// Previous pose:
    Sxya previous_pos_; 

    /// Current velocity:
    Sxya velocity_;  // [m/s]

    /// Previous velocity:
    Sxya previous_vel_;  // [m/s]

    //
    std::vector<Sxya> pivot_vel_; 

    //   
    std::vector<double> pivot_pos_; 

    //   
    std::vector<double> previous_pivot_pos_; 
     
    /// Previous wheel position/state [rad]:
};
}


#endif /* ODOMETRY_H_ */
